
#include "cv.h"
#include "cxmisc.h"
#include "highgui.h"
#include "cvaux.h"
#include <vector>
#include <string>
#include <algorithm>
#include <stdio.h>
#include <ctype.h>

//using namespace std;

int main(void) {

CvCapture* capture1=0;
CvCapture* capture2=0;
char key = 's';
CvSize imageSize = {640,480};
IplImage* img1=cvLoadImage("left.png",1);
IplImage* img2=cvLoadImage("right.png",1);
CvStereoBMState *BMState = cvCreateStereoBMState(CV_STEREO_BM_BASIC,0);
         assert(BMState != 0);
         BMState->preFilterSize=41;
         BMState->preFilterCap=31;
         BMState->SADWindowSize=31;
         BMState->minDisparity=-64;
         BMState->numberOfDisparities=128;
         BMState->textureThreshold=10;
         BMState->uniquenessRatio=15;

IplImage* frame1=0;
IplImage* frame2=0;
         while(1)
        {
             capture1 = cvCaptureFromCAM(1);
      if(!capture1){
            printf("Capture failure\n");
            return -1;
      }
             frame1=   cvQueryFrame( capture1 );
             if(!frame1) break;

            frame1=cvCloneImage(frame1);
            cvSmooth(frame1, frame1, CV_GAUSSIAN,3,3,0,0); //smooth the original image using Gaussian kernel

           IplImage* imgHSV = cvCreateImage(cvGetSize(frame1), IPL_DEPTH_8U, 3);
            //cvCvtColor(frame1, imgHSV, CV_BGR2HSV); //Change the color format from BGR to HSV
             IplImage* imgGrayScale = cvCreateImage(cvGetSize(frame1), IPL_DEPTH_8U, 3);
            //cvCvtColor(frame1, imgHSV, CV_BGR2GRAY);
            //frame2=cvQueryFrame( capture2 );
            //cvCvtColor(frame2, img2, CV_BGR2GRAY);


            // preparing matrices
CvMat* img1r = cvCreateMat( imageSize.height,
    imageSize.width, CV_8UC1 );
CvMat* img2r = cvCreateMat( imageSize.height,
    imageSize.width, CV_8UC1 );
CvMat* disp = cvCreateMat( imageSize.height,
    imageSize.width, CV_16S );
CvMat* vdisp = cvCreateMat( imageSize.height,
    imageSize.width, CV_8U );
CvMat* img1rnew = cvCreateMat( imageSize.height,
    imageSize.width, CV_8UC1 );
CvMat* img2rnew = cvCreateMat( imageSize.height,
    imageSize.width, CV_8UC1 );

//loading in mapx and mapy xml
CvMat *mx1 = (CvMat *)cvLoad("mx1.xml",NULL,NULL,NULL);
CvMat *my1 = (CvMat *)cvLoad("my1.xml",NULL,NULL,NULL);
CvMat *mx2 = (CvMat *)cvLoad("mx2.xml",NULL,NULL,NULL);
CvMat *my2 = (CvMat *)cvLoad("my2.xml",NULL,NULL,NULL);

//loop till 'q' is entered

    frame1= cvQueryFrame(capture1);
    frame2 = cvQueryFrame(capture2);

    IplImage* img1=frame1;
    IplImage* img2=frame2;

    // cloning img for remap
    img1r=(CvMat *)cvCloneImage(img1);
    img2r=(CvMat *)cvCloneImage(img2);

           if( img1 && img2 )
          {
                 CvMat part;
                 cvRemap( img1, img1r, mx1, my1 ,CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS,cvScalarAll(0));
                 cvRemap( img2, img2r, mx2, my2 ,CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS,cvScalarAll(0));
                 cvFindStereoCorrespondenceBM( img1r, img2r,
disp,BMState);
                 cvNormalize( disp, vdisp, 0, 256, CV_MINMAX,0 );
                 cvNamedWindow( "disparity",CV_WINDOW_AUTOSIZE );
                  //cvSaveImage("desert.jpg",vdisp,0);
                  cvShowImage("disparity",vdisp);

           }


        }
        // housekeeping
cvReleaseStereoBMState(&BMState);


cvReleaseCapture(&capture1);
cvReleaseCapture(&capture2);
return(0);
}

